//
// Created by javier on 2/11/15.
//

#include "gps_l1_ca_sim_channel_cc.h"
#include "geodetic.h"
#include <cmath>
#include <utility>


/* !\brief generate the C/A code sequence for a given Satellite Vehicle PRN
*/
void gps_l1_ca_sim_channel_cc::codegen()
{
    const unsigned int _code_length = 1023;
    bool G1[_code_length];
    bool G2[_code_length];
    bool G1_register[10], G2_register[10];
    bool feedback1, feedback2;
    bool aux;
    unsigned int lcv, lcv2;
    unsigned int delay;
    signed int prn_idx;

    /* G2 Delays as defined in GPS-ISD-200D */
    const signed int delays[51] = {5 /*PRN1*/, 6, 7, 8, 17, 18, 139, 140, 141, 251, 252, 254, 255, 256, 257, 258, 469, 470, 471, 472,
        473, 474, 509, 512, 513, 514, 515, 516, 859, 860, 861, 862 /*PRN32*/,
        145 /*PRN120*/, 175, 52, 21, 237, 235, 886, 657, 634, 762,
        355, 1012, 176, 603, 130, 359, 595, 68, 386 /*PRN138*/};

    // compute delay array index for given PRN number
    if (120 <= d_eph.i_satellite_PRN && d_eph.i_satellite_PRN <= 138)
        {
            prn_idx = d_eph.i_satellite_PRN - 88;  // SBAS PRNs are at array indices 31 to 50 (offset: -120+33-1 =-88)
        }
    else
        {
            prn_idx = d_eph.i_satellite_PRN - 1;
        }

    /* A simple error check */
    if ((prn_idx < 0) || (prn_idx > 51))
        {
            return;
        }

    for (lcv = 0; lcv < 10; lcv++)
        {
            G1_register[lcv] = true;
            G2_register[lcv] = true;
        }

    /* Generate G1 & G2 Register */
    for (lcv = 0; lcv < _code_length; lcv++)
        {
            G1[lcv] = G1_register[0];
            G2[lcv] = G2_register[0];

            feedback1 = G1_register[7] ^ G1_register[0];
            feedback2 = (bool)((G2_register[8] + G2_register[7] + G2_register[4] + G2_register[2] + G2_register[1] + G2_register[0]) & 0x1);

            for (lcv2 = 0; lcv2 < 9; lcv2++)
                {
                    G1_register[lcv2] = G1_register[lcv2 + 1];
                    G2_register[lcv2] = G2_register[lcv2 + 1];
                }

            G1_register[9] = feedback1;
            G2_register[9] = feedback2;
        }

    /* Set the delay */
    delay = _code_length - delays[prn_idx];
    delay %= _code_length;

    /* Generate PRN from G1 and G2 Registers */
    for (lcv = 0; lcv < _code_length; lcv++)
        {
            aux = G1[(lcv) % _code_length] ^ G2[delay];
            if (aux == true)
                {
                    //_dest[lcv] = std::complex<float>(1, 0);
                    d_code_table[lcv] = 1;
                }
            else
                {
                    //_dest[lcv] = std::complex<float>(-1, 0);
                    d_code_table[lcv] = 0;  //todo: enable -1 to gain 3db
                }
            delay++;
            delay %= _code_length;
        }

    //std::cout<<"d_eph.i_satellite_PRN ="<<d_eph.i_satellite_PRN <<"->"<<std::endl;
    //for (int n=0;n<_code_length;n++)
    //{
    //    std::cout<<","<<(int)d_code_table[n];

    //}
    //std::cout<<std::endl;
}


/*! \brief Compute Subframe from Ephemeris
 *  \param[in] eph Ephemeris of given SV
 *  \param[out] sbf Array of five sub-frames, 10 long words each
 */
void gps_l1_ca_sim_channel_cc::eph2sbf()
{
    unsigned long wn;
    unsigned long toe;
    unsigned long toc;
    unsigned long iode;
    unsigned long iodc;
    long deltan;
    long cuc;
    long cus;
    long cic;
    long cis;
    long crc;
    long crs;
    unsigned long ecc;
    unsigned long sqrta;
    long m0;
    long omg0;
    long inc0;
    long aop;
    long omgdot;
    long idot;
    long af0;
    long af1;
    long af2;
    long tgd;

    unsigned long ura = 2UL;
    unsigned long dataId = 1UL;
    unsigned long sbf4_page25_svId = 63UL;
    unsigned long sbf5_page25_svId = 51UL;

    unsigned long wna;
    unsigned long toa;

    wn = (unsigned long)(d_eph.d_Toe_gpstime.week % 1024);
    toe = (unsigned long)(d_eph.d_Toe_gpstime.sec / 16.0);
    toc = (unsigned long)(d_eph.d_Toc_gpstime.sec / 16.0);
    iode = (unsigned long)d_eph.d_IODE_SF2;
    iodc = (unsigned long)d_eph.d_IODC;
    deltan = (long)(d_eph.d_Delta_n / TWO_N43 / GPS_PI);
    cuc = (long)(d_eph.d_Cuc / TWO_N29);
    cus = (long)(d_eph.d_Cus / TWO_N29);
    cic = (long)(d_eph.d_Cic / TWO_N29);
    cis = (long)(d_eph.d_Cis / TWO_N29);
    crc = (long)(d_eph.d_Crc / TWO_N5);
    crs = (long)(d_eph.d_Crs / TWO_N5);
    ecc = (unsigned long)(d_eph.d_e_eccentricity / TWO_N33);
    sqrta = (unsigned long)(d_eph.d_sqrt_A / TWO_N19);
    m0 = (long)(d_eph.d_M_0 / TWO_N31 / GPS_PI);
    omg0 = (long)(d_eph.d_OMEGA0 / TWO_N31 / GPS_PI);
    inc0 = (long)(d_eph.d_i_0 / TWO_N31 / GPS_PI);
    aop = (long)(d_eph.d_OMEGA / TWO_N31 / GPS_PI);
    omgdot = (long)(d_eph.d_OMEGA_DOT / TWO_N43 / GPS_PI);
    idot = (long)(d_eph.d_IDOT / TWO_N43 / GPS_PI);
    af0 = (long)(d_eph.d_A_f0 / TWO_N31);
    af1 = (long)(d_eph.d_A_f1 / TWO_N43);
    af2 = (long)(d_eph.d_A_f2 / TWO_N55);
    tgd = (long)(d_eph.d_TGD / TWO_N31);

    wna = (unsigned long)(d_eph.d_Toe_gpstime.week % 256);
    toa = (unsigned long)(d_eph.d_Toe_gpstime.sec / 4096.0);

    // Subframe 1
    d_sbf[0][0] = 0x8B0000UL << 6;
    d_sbf[0][1] = 0x1UL << 8;
    d_sbf[0][2] = ((wn & 0x3FFUL) << 20) | (ura << 14) | (((iodc >> 8) & 0x3UL) << 6);
    d_sbf[0][3] = 0UL;
    d_sbf[0][4] = 0UL;
    d_sbf[0][5] = 0UL;
    d_sbf[0][6] = (tgd & 0xFFUL) << 6;
    d_sbf[0][7] = ((iodc & 0xFFUL) << 22) | ((toc & 0xFFFFUL) << 6);
    d_sbf[0][8] = ((af2 & 0xFFUL) << 22) | ((af1 & 0xFFFFUL) << 6);
    d_sbf[0][9] = (af0 & 0x3FFFFFUL) << 8;

    // Subframe 2
    d_sbf[1][0] = 0x8B0000UL << 6;
    d_sbf[1][1] = 0x2UL << 8;
    d_sbf[1][2] = ((iode & 0xFFUL) << 22) | ((crs & 0xFFFFUL) << 6);
    d_sbf[1][3] = ((deltan & 0xFFFFUL) << 14) | (((m0 >> 24) & 0xFFUL) << 6);
    d_sbf[1][4] = (m0 & 0xFFFFFFUL) << 6;
    d_sbf[1][5] = ((cuc & 0xFFFFUL) << 14) | (((ecc >> 24) & 0xFFUL) << 6);
    d_sbf[1][6] = (ecc & 0xFFFFFFUL) << 6;
    d_sbf[1][7] = ((cus & 0xFFFFUL) << 14) | (((sqrta >> 24) & 0xFFUL) << 6);
    d_sbf[1][8] = (sqrta & 0xFFFFFFUL) << 6;
    d_sbf[1][9] = (toe & 0xFFFFUL) << 14;

    // Subframe 3
    d_sbf[2][0] = 0x8B0000UL << 6;
    d_sbf[2][1] = 0x3UL << 8;
    d_sbf[2][2] = ((cic & 0xFFFFUL) << 14) | (((omg0 >> 24) & 0xFFUL) << 6);
    d_sbf[2][3] = (omg0 & 0xFFFFFFUL) << 6;
    d_sbf[2][4] = ((cis & 0xFFFFUL) << 14) | (((inc0 >> 24) & 0xFFUL) << 6);
    d_sbf[2][5] = (inc0 & 0xFFFFFFUL) << 6;
    d_sbf[2][6] = ((crc & 0xFFFFUL) << 14) | (((aop >> 24) & 0xFFUL) << 6);
    d_sbf[2][7] = (aop & 0xFFFFFFUL) << 6;
    d_sbf[2][8] = (omgdot & 0xFFFFFFUL) << 6;
    d_sbf[2][9] = ((iode & 0xFFUL) << 22) | ((idot & 0x3FFFUL) << 8);

    // Subframe 4, page 25
    d_sbf[3][0] = 0x8B0000UL << 6;
    d_sbf[3][1] = 0x4UL << 8;
    d_sbf[3][2] = (dataId << 28) | (sbf4_page25_svId << 22);
    d_sbf[3][3] = 0UL;
    d_sbf[3][4] = 0UL;
    d_sbf[3][5] = 0UL;
    d_sbf[3][6] = 0UL;
    d_sbf[3][7] = 0UL;
    d_sbf[3][8] = 0UL;
    d_sbf[3][9] = 0UL;

    // Subframe 5, page 25
    d_sbf[4][0] = 0x8B0000UL << 6;
    d_sbf[4][1] = 0x5UL << 8;
    d_sbf[4][2] = (dataId << 28) | (sbf5_page25_svId << 22) | ((toa & 0xFFUL) << 14) | ((wna & 0xFFUL) << 6);
    d_sbf[4][3] = 0UL;
    d_sbf[4][4] = 0UL;
    d_sbf[4][5] = 0UL;
    d_sbf[4][6] = 0UL;
    d_sbf[4][7] = 0UL;
    d_sbf[4][8] = 0UL;
    d_sbf[4][9] = 0UL;

    return;
}

/*! \brief Count number of bits set to 1
 *  \param[in] v long word in whihc bits are counted
 *  \returns Count of bits set to 1
 */
unsigned long gps_l1_ca_sim_channel_cc::countBits(unsigned long v)
{
    unsigned long c;
    const int S[] = {1, 2, 4, 8, 16};
    const unsigned long B[] = {
        0x55555555, 0x33333333, 0x0F0F0F0F, 0x00FF00FF, 0x0000FFFF};

    c = v;
    c = ((c >> S[0]) & B[0]) + (c & B[0]);
    c = ((c >> S[1]) & B[1]) + (c & B[1]);
    c = ((c >> S[2]) & B[2]) + (c & B[2]);
    c = ((c >> S[3]) & B[3]) + (c & B[3]);
    c = ((c >> S[4]) & B[4]) + (c & B[4]);

    return (c);
}

/*! \brief Compute the Checksum for one given word of a subframe
 *  \param[in] source The input data
 *  \param[in] nib Does this word contain non-information-bearing bits?
 *  \returns Computed Checksum
 */
unsigned long gps_l1_ca_sim_channel_cc::computeChecksum(unsigned long source, int nib)
{
    /*
    Bits 31 to 30 = 2 LSBs of the previous transmitted word, D29* and D30*
    Bits 29 to  6 = Source data bits, d1, d2, ..., d24
    Bits  5 to  0 = Empty parity bits
    */

    /*
    Bits 31 to 30 = 2 LSBs of the previous transmitted word, D29* and D30*
    Bits 29 to  6 = Data bits transmitted by the SV, D1, D2, ..., D24
    Bits  5 to  0 = Computed parity bits, D25, D26, ..., D30
    */

    /*
                      1            2           3
    bit    12 3456 7890 1234 5678 9012 3456 7890
    ---    -------------------------------------
    D25    11 1011 0001 1111 0011 0100 1000 0000
    D26    01 1101 1000 1111 1001 1010 0100 0000
    D27    10 1110 1100 0111 1100 1101 0000 0000
    D28    01 0111 0110 0011 1110 0110 1000 0000
    D29    10 1011 1011 0001 1111 0011 0100 0000
    D30    00 1011 0111 1010 1000 1001 1100 0000
    */

    unsigned long bmask[6] = {
        0x3B1F3480UL, 0x1D8F9A40UL, 0x2EC7CD00UL,
        0x1763E680UL, 0x2BB1F340UL, 0x0B7A89C0UL};

    unsigned long D;
    unsigned long d = source & 0x3FFFFFC0UL;
    unsigned long D29 = (source >> 31) & 0x1UL;
    unsigned long D30 = (source >> 30) & 0x1UL;

    if (nib)  // Non-information bearing bits for word 2 and 10
        {
            /*
        Solve bits 23 and 24 to presearve parity check
        with zeros in bits 29 and 30.
        */

            if ((D30 + countBits(bmask[4] & d)) % 2)
                {
                    d ^= (0x1UL << 6);
                }
            if ((D29 + countBits(bmask[5] & d)) % 2)
                {
                    d ^= (0x1UL << 7);
                }
        }

    D = d;
    if (D30)
        {
            D ^= 0x3FFFFFC0UL;
        }

    D |= ((D29 + countBits(bmask[0] & d)) % 2) << 5;
    D |= ((D30 + countBits(bmask[1] & d)) % 2) << 4;
    D |= ((D29 + countBits(bmask[2] & d)) % 2) << 3;
    D |= ((D30 + countBits(bmask[3] & d)) % 2) << 2;
    D |= ((D30 + countBits(bmask[4] & d)) % 2) << 1;
    D |= ((D29 + countBits(bmask[5] & d)) % 2);

    D &= 0x3FFFFFFFUL;
    //D |= (source & 0xC0000000UL); // Add D29* and D30* from source data bits

    return (D);
}


/*! \brief Compute the code phase for a given channel (satellite)
 *  \param chan Channel on which we operate (is updated)
 *  \param[in] rho0 Range at start of interval
 *  \param[in] rho1 Current range, after \a dt has expired
 *  \param[in dt delta-t (time difference) in seconds
 *  \return current TOW of this interval
 */
double gps_l1_ca_sim_channel_cc::computeCodePhase(pseudorange rho1, gpstime g0, double dt)
{
    double ms;
    int ims;
    double rhorate;

    // Pseudorange rate.
    rhorate = (rho1.range - d_rho0.range) / dt;

    // Carrier and code frequency.
    d_carrier_freq = -rhorate / GPS_L1_LAMBDA;
    d_carrier_freq_l2 = -rhorate / GPS_L2_LAMBDA;
    d_code_freq = GPS_L1_CA_CODE_RATE_HZ + d_carrier_freq * GPS_L1_CA_CARR_TO_CODE;

    // Initial code phase and data bit counters.
    ms = (((d_rho0.g.sec - g0.sec) + 6.0) - d_rho0.range / GPS_C_m_s) * 1000.0;
    ims = (int)ms;

    double current_TOW = ms / 1000.0 + g0.sec - 6.0;

    d_code_phase = (ms - (double)ims) * GPS_L1_CA_CODE_LENGTH_CHIPS;  // in chip

    d_iword = ims / 600;  // 1 word = 30 bits = 600 ms
    ims -= d_iword * 600;

    d_ibit = ims / 20;  // 1 bit = 20 code = 20 ms
    ims -= d_ibit * 20;

    d_icode = ims;  // 1 code = 1 ms

    d_current_chip = d_code_table[(int)d_code_phase] * 2 - 1;
    d_dataBit = (int)((d_tlm_words[d_iword] >> (29 - d_ibit)) & 0x1UL) * 2 - 1;

    return current_TOW;
}

void gps_l1_ca_sim_channel_cc::update_AOA(std::vector<double> obs_xyz, gpstime grx)
{
    double xyz[3];
    double llh[3];
    double tmat[3][3];
    double pos[3];
    double vel[3];
    double clk[2];
    double los[3];
    double neu[3];
    double azel[2];
    double elvmask = 10.0;

    xyz[0] = obs_xyz[0];
    xyz[1] = obs_xyz[1];
    xyz[2] = obs_xyz[2];

    xyz2llh(xyz, llh);  //in RADIANS!
    ltcmat(llh, tmat);

    if (d_eph.i_satellite_PRN != 0)
        {
            satpos(&d_eph, grx, pos, vel, clk);
            //std::cout.precision(15);
            //std::cout<<"SAT "<<d_eph.i_satellite_PRN<<" grx.sec="<<grx.sec<<"pos="<<pos[0]<<","<<pos[1]<<","<<pos[2]<<std::endl;
            subVect(los, pos, xyz);
            ecef2neu(los, tmat, neu);
            neu2azel(azel, neu);

            d_az_deg = azel[0] * GPS_RAD2DEG;
            d_el_deg = azel[1] * GPS_RAD2DEG;
            //if (d_el_deg>elvmask)
            //{
            //    std::cout<<"SAT "<<d_eph.i_satellite_PRN<<" Az="<<d_az_deg<<", El="<<d_el_deg<<std::endl;
            //}
        }
}
//update observables and report current TOW of the received signal
void gps_l1_ca_sim_channel_cc::update_observables(std::vector<double> obs_xyz, gpstime grx, gpstime g0)
{
    ////////////////////////////////////////////////////////////
    // Check visible satellites
    ////////////////////////////////////////////////////////////

    double xyz[3];

    update_AOA(obs_xyz, grx);
    // Refresh code phase and data bit counters
    pseudorange rho;

    // Current pseudorange
    xyz[0] = obs_xyz[0];
    xyz[1] = obs_xyz[1];
    xyz[2] = obs_xyz[2];
    computeRange(&rho, &d_eph, grx, xyz);
    //std::cout.precision(15);
    //std::cout<<"nsat="<<d_eph.i_satellite_PRN<<" grx.sec="<<grx.sec<<"rho="<<rho.range<<"xyz[0]="<<xyz[0]<<"xyz[1]="<<xyz[1]<<"xyz[2]="<<xyz[2]<<std::endl;
    // Update code phase and data bit counters
    // Get the current TOW of the received signal for the start of the current interval
    d_current_TOW = computeCodePhase(rho, g0, 0.1);

    // Save current pseudorange
    d_rho0 = rho;
}

void gps_l1_ca_sim_channel_cc::generate_signal()
{
    //todo: Use Volk operators here
    for (int isamp = 0; isamp < d_buffer_size; isamp++)
        {
            float signal_I;
            float signal_Q;

            signal_I = (static_cast<float>(d_dataBit) * static_cast<float>(d_current_chip) * static_cast<float>(cos(GPS_TWO_PI * d_carrier_phase)));
            signal_Q = (static_cast<float>(d_dataBit) * static_cast<float>(d_current_chip) * static_cast<float>(sin(GPS_TWO_PI * d_carrier_phase)));

            if (d_enable_noise)
                {
                    signal_I *= d_signal_amplitude_lin;
                    signal_Q *= d_signal_amplitude_lin;
                }

            // Store I/Q samples into buffer
            d_output_buffer[isamp] = std::complex<float>(signal_I, signal_Q);

            //if(isamp==0)
            //{
            //    std::cout<<"ip="<<d_output_buffer[isamp].real()<<" qp="<<d_output_buffer[isamp].imag()<<" d_current_chip="<<(int)d_current_chip<<" d_carrier_phase="<<d_carrier_phase<<" d_dataBit="<<(int)d_dataBit<<std::endl;
            //}

            // Update code phase
            d_code_phase += d_code_freq * d_sampling_time;

            if (d_code_phase >= GPS_L1_CA_CODE_LENGTH_CHIPS)
                {
                    d_code_phase -= GPS_L1_CA_CODE_LENGTH_CHIPS;

                    d_icode++;

                    if (d_icode >= 20)  // 20 C/A codes = 1 navigation data bit
                        {
                            d_icode = 0;
                            d_ibit++;

                            if (d_ibit >= 30)  // 30 navigation data bits = 1 word
                                {
                                    d_ibit = 0;
                                    d_iword++;
                                }

                            // Set new navigation data bit
                            d_dataBit = (int8_t)(((d_tlm_words[d_iword] >> (29 - d_ibit)) & 0x1UL) * 2 - 1);
                        }
                }

            // Set currnt code chip
            d_current_chip = d_code_table[(int)d_code_phase] * 2 - 1;  //todo: depend on the value of the generator (1,0) or (1,-1)

            // Update carrier phase
            d_carrier_phase += d_carrier_freq * d_sampling_time;


            // wrap phase between {0,1} -> {0,2PI} in NCO
            if (d_carrier_phase >= 1.0)
                {
                    d_carrier_phase -= 1.0;
                }
            else if (d_carrier_phase < 0.0)
                {
                    d_carrier_phase += 1.0;
                }
        }
    // accumulated carrier phase (for rinex obs)
    //RINEX format consistency:
    //The phase changes in the same sense as the range (negative doppler)
    d_acc_carrier_phase_cycles -= d_carrier_freq * d_sampling_time * d_buffer_size;
    d_acc_carrier_phase_l2_cycles -= d_carrier_freq_l2 * d_sampling_time * d_buffer_size;
    //accumulate current timestamp
    d_current_timestamp_s += d_sampling_time * d_buffer_size;
}

void gps_l1_ca_sim_channel_cc::satpos(Gps_Ephemeris *eph, gpstime g, double *pos, double *vel, double *clk)
{
    // Computing Satellite Velocity using the Broadcast Ephemeris
    // http://www.ngs.noaa.gov/gps-toolbox/bc_velo.htm

    double tk;
    double mk;
    double ek;
    double ekold;
    double ekdot;
    double cek, sek;
    double pk;
    double pkdot;
    double c2pk, s2pk;
    double uk;
    double ukdot;
    double cuk, suk;
    double ok;
    double sok, cok;
    double ik;
    double ikdot;
    double sik, cik;
    double rk;
    double rkdot;
    double xpk, ypk;
    double xpkdot, ypkdot;

    double relativistic, OneMinusecosE, tmp;

    tk = g.sec - eph->d_Toe_gpstime.sec;

    if (tk > 302400.0)
        {                    //SECONDS_IN_HALF_WEEK
            tk -= 604800.0;  //SECONDS_IN_WEEK
        }
    else if (tk < -302400.0)
        {                    //SECONDS_IN_HALF_WEEK
            tk += 604800.0;  //SECONDS_IN_WEEK
        }

    // Update the working variables
    double A;
    A = eph->d_sqrt_A * eph->d_sqrt_A;
    double n;
    n = sqrt(GM / (A * A * A)) + eph->d_Delta_n;
    double sq1e2;
    sq1e2 = sqrt(1.0 - eph->d_e_eccentricity * eph->d_e_eccentricity);
    double omgkdot;
    omgkdot = eph->d_OMEGA_DOT - OMEGA_EARTH_DOT;

    mk = eph->d_M_0 + n * tk;
    ek = mk;
    ekold = ek + 1.0;

    OneMinusecosE = 0;  // Suppress the uninitialized warning.
    while (fabs(ek - ekold) > 1.0E-14)
        {
            ekold = ek;
            OneMinusecosE = 1.0 - eph->d_e_eccentricity * cos(ekold);
            ek = ek + (mk - ekold + eph->d_e_eccentricity * sin(ekold)) / OneMinusecosE;
        }

    sek = sin(ek);
    cek = cos(ek);

    ekdot = n / OneMinusecosE;

    relativistic = -4.442807633E-10 * eph->d_e_eccentricity * eph->d_sqrt_A * sek;

    pk = atan2(sq1e2 * sek, cek - eph->d_e_eccentricity) + eph->d_OMEGA;
    pkdot = sq1e2 * ekdot / OneMinusecosE;

    s2pk = sin(2.0 * pk);
    c2pk = cos(2.0 * pk);

    uk = pk + eph->d_Cus * s2pk + eph->d_Cuc * c2pk;
    suk = sin(uk);
    cuk = cos(uk);
    ukdot = pkdot * (1.0 + 2.0 * (eph->d_Cus * c2pk - eph->d_Cuc * s2pk));

    rk = A * OneMinusecosE + eph->d_Crc * c2pk + eph->d_Crs * s2pk;
    rkdot = A * eph->d_e_eccentricity * sek * ekdot + 2.0 * pkdot * (eph->d_Crs * c2pk - eph->d_Crc * s2pk);

    ik = eph->d_i_0 + eph->d_IDOT * tk + eph->d_Cic * c2pk + eph->d_Cis * s2pk;
    sik = sin(ik);
    cik = cos(ik);
    ikdot = eph->d_IDOT + 2.0 * pkdot * (eph->d_Cis * c2pk - eph->d_Cic * s2pk);

    xpk = rk * cuk;
    ypk = rk * suk;
    xpkdot = rkdot * cuk - ypk * ukdot;
    ypkdot = rkdot * suk + xpk * ukdot;

    ok = eph->d_OMEGA0 + tk * omgkdot - OMEGA_EARTH_DOT * eph->d_Toe_gpstime.sec;

    //std::cout<<"d_OMEGA0="<<eph->d_OMEGA0<<" omgkdot="<<omgkdot<<" Toe sec="<<eph->d_Toe_gpstime.sec<<std::endl;

    sok = sin(ok);
    cok = cos(ok);

    pos[0] = xpk * cok - ypk * cik * sok;
    pos[1] = xpk * sok + ypk * cik * cok;
    pos[2] = ypk * sik;

    tmp = ypkdot * cik - ypk * sik * ikdot;

    vel[0] = -omgkdot * pos[1] + xpkdot * cok - tmp * sok;
    vel[1] = omgkdot * pos[0] + xpkdot * sok + tmp * cok;
    vel[2] = ypk * cik * ikdot + ypkdot * sik;

    // Satellite clock correction
    tk = g.sec - eph->d_Toc_gpstime.sec;

    if (tk > 302400.0)
        {                    //SECONDS_IN_HALF_WEEK
            tk -= 604800.0;  //SECONDS_IN_WEEK
        }
    else if (tk < -302400.0)
        {                    //SECONDS_IN_HALF_WEEK
            tk += 604800.0;  //SECONDS_IN_WEEK
        }


    clk[0] = eph->d_A_f0 + tk * (eph->d_A_f1 + tk * eph->d_A_f2) + relativistic - eph->d_TGD;
    clk[1] = eph->d_A_f1 + 2.0 * tk * eph->d_A_f2;


    //std::cout.precision(15);
    //std::cout<<"SAT "<<d_eph.i_satellite_PRN<<"tk="<<tk<<"mk="<<mk<<"ek="<<ek<<"ekold="<<ekold<<"ekdot="<<ekdot
    //<<"cek="<<cek<<"sek="<<sek<<"pk="<<pk<<"pkdot="<<pkdot<<"c2pk="<<c2pk<<"s2pk="<<s2pk<<"uk="<<uk<<"ukdot="<<ukdot
    //<<"cuk="<<cuk<<"suk="<<suk<<"ok="<<ok<<"sok="<<sok<<"cok="<<cok<<"ik="<<ik<<"ikdot="<<ikdot<<"sik="<<sik
    //<<"cik="<<cik<<"rk="<<rk<<"rkdot="<<rkdot<<"xpk="<<ypk<<"ypk="<<xpkdot<<"xpkdot="<<ypkdot<<std::endl;


    return;
}

void gps_l1_ca_sim_channel_cc::subVect(double *y, const double *x1, const double *x2)
{
    y[0] = x1[0] - x2[0];
    y[1] = x1[1] - x2[1];
    y[2] = x1[2] - x2[2];

    return;
}

double gps_l1_ca_sim_channel_cc::normVect(const double *x)
{
    return (sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]));
}

double gps_l1_ca_sim_channel_cc::dotProd(const double *x1, const double *x2)
{
    return (x1[0] * x2[0] + x1[1] * x2[1] + x1[2] * x2[2]);
}

void gps_l1_ca_sim_channel_cc::computeRange(pseudorange *rho, Gps_Ephemeris *eph, gpstime g, double *xyz)
{
    double pos[3], vel[3], clk[2];
    double los[3];
    double tau;
    double range, rate;
    double xrot, yrot;

    // SV position at time of the pseudorange observation.
    satpos(eph, g, pos, vel, clk);

    //std::cout.precision(15);
    //std::cout<<"SAT "<<d_eph.i_satellite_PRN<<"g.week="<<g.week<<"g.sec="<<g.sec<<std::endl;
    //std::cout<<"SAT "<<d_eph.i_satellite_PRN<<"pos="<<pos[0]<<","<<pos[1]<<","<<pos[2]<<std::endl;
    //std::cout<<"SAT "<<d_eph.i_satellite_PRN<<"vel="<<vel[0]<<","<<vel[1]<<","<<vel[2]<<std::endl;
    //std::cout<<"SAT "<<d_eph.i_satellite_PRN<<"clk="<<clk[0]<<","<<clk[1]<<std::endl;

    // Receiver to satellite vector and light-time.
    subVect(los, pos, xyz);
    tau = normVect(los) / GPS_C_m_s;

    // Extrapolate the satellite position backwards to the transmission time.
    pos[0] -= vel[0] * tau;
    pos[1] -= vel[1] * tau;
    pos[2] -= vel[2] * tau;

    // Earth rotation correction. The change in velocity can be neglected.
    xrot = pos[0] + pos[1] * OMEGA_EARTH_DOT * tau;
    yrot = pos[1] - pos[0] * OMEGA_EARTH_DOT * tau;
    pos[0] = xrot;
    pos[1] = yrot;

    //std::cout<<"pos2="<<pos[0]<<","<<pos[1]<<","<<pos[2]<<std::endl;
    // New observer to satellite vector and satellite range.
    subVect(los, pos, xyz);
    range = normVect(los);

    //true geometric range for receiver testing
    rho->true_range = range;
    // Pseudorange.
    rho->range = range - GPS_C_m_s * clk[0];

    // Relative velocity of SV and receiver.
    rate = dotProd(vel, los) / range;

    // true geometric range rate for receiver testing.
    rho->true_rate = rate;

    // Pseudorange rate.
    const double SPEED_OF_LIGHT = 299792458.0;  //!<  [m/s]
    rho->rate = rate - SPEED_OF_LIGHT * clk[1];

    // Time of application
    rho->g = g;

    return;
}

gps_l1_ca_sim_channel_cc::gps_l1_ca_sim_channel_cc(Gps_Ephemeris eph, double sampling_freq)
{
    d_eph = std::move(eph);
    d_sampling_freq = sampling_freq;
    d_sampling_time = 1 / sampling_freq;
    d_code_table = new int8_t[GPS_L1_CA_CODE_LENGTH_CHIPS_INT];
    //todo: This is a limitation in the simulation duration!!!
    d_tlm_words = new unsigned long[51 * 10];  // 51 subframes (306 secs) * 10 words per subframe
    d_acc_carrier_phase_cycles = 0.0;
    d_acc_carrier_phase_l2_cycles = 0.0;
    d_current_timestamp_s = 0.0;
    d_enable_noise = false;
    d_signal_amplitude_lin = 1.0;
}

gps_l1_ca_sim_channel_cc::~gps_l1_ca_sim_channel_cc()
{
    delete d_code_table;
}

void gps_l1_ca_sim_channel_cc::set_output_buffer(std::complex<float> *out_buffer, int size)
{
    d_output_buffer = out_buffer;
    d_buffer_size = size;
}

void gps_l1_ca_sim_channel_cc::set_cn0(bool enable_noise, double signal_amplitude_lin)
{
    d_enable_noise = enable_noise;
    d_signal_amplitude_lin = signal_amplitude_lin;
}

void gps_l1_ca_sim_channel_cc::generate_telemetry(gpstime g0)
{
    //todo: Add simulation duration parameter. there is a limitation of 300 seconds!
    int N_SBF = 51;       //51 subframes (6 * 51 = 306 seconds) N_SBF
    int N_DWRD_SBF = 10;  //10 words per subframe N_DWRD_SBF
    eph2sbf();
    // Data bit reference time

    unsigned long sbfwrd;
    int nib;
    unsigned long tow = ((unsigned long)g0.sec) / 6UL;
    unsigned long prevwrd = 0UL;

    for (int isbf = 0; isbf < N_SBF; isbf++)
        {
            for (int iwrd = 0; iwrd < N_DWRD_SBF; iwrd++)
                {
                    sbfwrd = d_sbf[(isbf + 4) % 5][iwrd];  // Start from subframe 5

                    // Add TOW-count message into HOW
                    if (iwrd == 1)
                        {
                            sbfwrd |= ((tow & 0x1FFFFUL) << 13);
                        }

                    // Compute checksum
                    sbfwrd |= (prevwrd << 30) & 0xC0000000UL;  // 2 LSBs of the previous transmitted word

                    nib = ((iwrd == 1) || (iwrd == 9)) ? 1 : 0;  // Non-information bearing bits for word 2 and 10

                    d_tlm_words[isbf * N_DWRD_SBF + iwrd] = computeChecksum(sbfwrd, nib);

                    prevwrd = d_tlm_words[isbf * N_DWRD_SBF + iwrd];
                }

            tow++;  // Next subframe
        }
}

void gps_l1_ca_sim_channel_cc::init_carrier_phase_and_range(gpstime grx, std::vector<double> obs_xyz)
{
    pseudorange tmp;
    double ref[3] = {0.0};
    double r_ref, r_xyz;
    double phase_ini;

    computeRange(&tmp, &d_eph, grx, ref);
    r_ref = tmp.range;

    double xyz[3];
    xyz[0] = obs_xyz[0];
    xyz[1] = obs_xyz[1];
    xyz[2] = obs_xyz[2];
    computeRange(&tmp, &d_eph, grx, xyz);

    r_xyz = tmp.range;

    phase_ini = (2.0 * r_ref - r_xyz) / GPS_L1_LAMBDA;

    d_carrier_phase = phase_ini - floor(phase_ini);

    //initial psudorange
    computeRange(&d_rho0, &d_eph, grx, xyz);
}

double gps_l1_ca_sim_channel_cc::get_pseudorange()
{
    return d_rho0.range;
}

double gps_l1_ca_sim_channel_cc::get_true_range()
{
    return d_rho0.true_range;
}


double gps_l1_ca_sim_channel_cc::get_acc_carrier_phase()
{
    return d_acc_carrier_phase_cycles;
}

double gps_l1_ca_sim_channel_cc::get_doppler()
{
    return d_carrier_freq;
}

double gps_l1_ca_sim_channel_cc::get_code_phase()
{
    return d_code_phase;
}

double gps_l1_ca_sim_channel_cc::get_current_timestamp()
{
    return d_current_timestamp_s;
}
double gps_l1_ca_sim_channel_cc::get_acc_carrier_phase_l2()
{
    return d_acc_carrier_phase_l2_cycles;
}
